#pragma once
#include <iostream>
#include <cmath>

#include "amr_trajectory_msgs/amr_trajectory_msg.h"

namespace common_lib
{
class TrapezoidalSpeedPlan
{
public:
    TrapezoidalSpeedPlan()
    {}
    ~TrapezoidalSpeedPlan()
    {}

    void TrapezoidalCalcSpeed(amr_trajectory_msgs::amr_trajectory_msg &trajectory, float acc, float dcc, float vs, float &ve, float &vmax, int sign = 1);

    void TrapezoidalCalcSpeed(std::vector<amr_trajectory_msgs::point_info> &points, int start, int end, float acc, float dcc, float vs, float &ve, float &vmax, int sign = 1);
};
};